perm filename TRAJ.SAI[SYS,HE]5 blob
sn#020541 filedate 1973-01-30 generic text, type T, neo UTF8
00100 DEFINE WRITE="DEB_HAND";
00200 REQUIRE "VECTOR.SAI[SYS,HE]" SOURCE_FILE;
00300 EXTERNAL INTEGER _SKIP_;
00400 DEFINE AVT (A, S, CA, SA)="1.0, -CA, SA, A, 1.0, CA, -SA, A, 0.0, SA, CA, S, [3] 0.0, 1.0, 0.0 ";
00500 DEFINE AVP (A, S, CA, SA)="-CA, SA, A, CA, -SA, A";
00600 DEFINE AVS (A, CA, SA, CT, ST)="CT, -CA*ST, SA*ST, A*CT, ST, CA*CT, -SA*CT, A*ST,
00700 0.0, SA, CA, 1.0, [3] 0.0, 1.0, 0.0 ";
00800 DEFINE QT = "0.0, -1.0, 0.0, 0.0, 1.0, [12] 0.0 ";
00900 DEFINE QS = "[11] 0.0, 1.0, [5] 0.0 ";
01000 DEFINE JDEF (M, X, Y, Z, IXX, IYY, IZZ) =".5*(-IXX+IYY+IZZ), 0.0, 0.0, M*X, 0.0,
01100 .5*(IXX-IYY+IZZ), 0.0, M*Y, 0.0, 0.0, .5*(IXX+IYY-IZZ), M*Z, M*X, M*Y, M*Z, M, 0.0 ";
01200 REQUIRE "YELLOW.SAI[SYS,HE]" SOURCE_FILE;
01300 SAFE REAL ARRAY U1,T[0:101];
01400 SAFE REAL ARRAY DIAG[1:13];
01500 SAFE INTEGER ARRAY COEFF[0:'1037];
01600 SAFE REAL ARRAY ARRIVE_ARM,DEPART_ARM[1:4];
01700 REAL F1_ARM,F2_ARM,OBJECT_MASS,OBJECT_KXX,OBJECT_KYY,OBJECT_KZZ;
01800 INTEGER T1_ARM,T2_ARM;
01900 DEFINE MAX_SEG="8";
02000 SAFE REAL ARRAY POSITION[1:6,0:MAX_SEG];
02100 SAFE REAL ARRAY KOE[1:6,1:(MAX_SEG*3)+6];
02200 SAFE INTEGER ARRAY PERIOD[1:6,1:MAX_SEG];
02300 SAFE INTEGER ARRAY NS[1:6];
02400 SAFE REAL ARRAY POS[0:MAX_SEG];
02500 SAFE REAL ARRAY SOLN[1:(MAX_SEG*3)+6];
02600 SAFE INTEGER ARRAY TIM[1:MAX_SEG];
02700 SAFE STRING ARRAY BANDS[0:'37];
02800 SAFE INTEGER ARRAY TRACK[0:'40];
02900 INTEGER NEXT_BAND,BAND,BAND_NUMBER;
03000 PRELOAD_WITH 0,'1040,0;
03100 SAFE INTEGER ARRAY GOODIE[1:3];
03200 DEFINE UFBWRT="'707000000000";
03300 BOOLEAN FAST;
03400 SAFE REAL ARRAY LAST_ARM[1:6];
03500 SAFE REAL ARRAY LAST_TRANS[1:4,1:4];
03600 SAFE REAL ARRAY QA,D[0:16];
03700 SAFE REAL ARRAY TF[1:6];
03800 SAFE REAL ARRAY LU,NR[1:6,1:6];
03900 REAL DIGITS;
04000 INTEGER LOOP,JOINT,DURATION;
04100 SAFE INTEGER ARRAY DATA_STACK[1:500];
04200 SAFE REAL ARRAY REAL_DATA[1:18];
04300 REQUIRE "MATRIX.REL[SYS,HE]" LOAD_MODULE;
04400 REQUIRE "SAILIB.REL[SYS,HE]" LOAD_MODULE;
04500 EXTERNAL SIMPLE PROCEDURE MMOVE(REFERENCE REAL R,A);
04600 EXTERNAL SIMPLE PROCEDURE MTIMES(REFERENCE REAL R,A,B);
04700 EXTERNAL REAL SIMPLE PROCEDURE TRACET(REFERENCE REAL X,Y);
04800 EXTERNAL SIMPLE PROCEDURE DECOMPOSE(INTEGER I; REAL ARRAY NR,LU);
04900 EXTERNAL SIMPLE PROCEDURE SOLVE(INTEGER I;REAL ARRAY NR,LU,DTH);
05000 EXTERNAL SIMPLE PROCEDURE IMPROVE(INTEGER I; REAL ARRAY NR,LU,DA,DTH;REFERENCE REAL DIGITS);
05100 INTEGER SIMPLE PROCEDURE SQAR (INTEGER I);
05200 RETURN ((I-1)*17);
05300
00100 SIMPLE PROCEDURE COMPLETA (INTEGER I;SAFE REAL ARRAY TH);
00200 BEGIN REAL S,C;
00300 INTEGER J,K;
00400 J←SQAR(I);
00500 K←6*(I-1);
00600 S←SIND(TH[I]);
00700 C←COSD(TH[I]);
00800 A[J]←C;
00900 A[J+1]←APAR[K]*S;
01000 A[J+2]←APAR[K+1]*S;
01100 A[J+3]←APAR[K+2]*C;
01200 A[J+4]←S;
01300 A[J+5]←APAR[K+3]*C;
01400 A[J+6]←APAR[K+4]*C;
01500 A[J+7]←APAR[K+5]*S;
01600 END;
01700
01800 SIMPLE PROCEDURE HANDPOS(SAFE REAL ARRAY TH);
01900 BEGIN INTEGER I;
02000 FOR I←1,2,4,5,6 DO COMPLETA(I,TH);
02100 A[SQAR(3)+11]←TH[3];
02200 A[3]←A[3]+SHOLDER[1];
02300 A[7]←A[7]+SHOLDER[2];
02400 MMOVE(A[0],A[0]);
02500 MMOVE(T[0],A[0]);
02600 UNDERFLOW(-1);
02700 FOR I←2 STEP 1 UNTIL 6 DO MTIMES(T[SQAR(I)],T[SQAR(I-1)],A[SQAR(I)]);
02800 UNDERFLOW(0);
02900 END;
03000
03100 SIMPLE PROCEDURE UPDATE_SEG;
03200 BEGIN INTEGER I;
03300 HANDPOS(ARM_VECTOR);
03400 FOR I←3 STEP 1 UNTIL 6 DO
03500 BEGIN
03600 ARRBLT(ARM_LINK[I,1,1],T[SQAR(I)],16);
03700 END;
03800 GRASP←ARM_VECTOR[7];
03900 END;
04000
04100 SIMPLE PROCEDURE NOT_LESS(REFERENCE REAL V;REAL L);
04200 BEGIN
04300 IF V<0.0 ∧ V>-L THEN BEGIN V←0; RETURN END;
04400 IF V≥0.0 ∧ V< L THEN V←0;
04500 END;
04600
00100 SIMPLE PROCEDURE INCREMENT(SAFE REAL ARRAY DTH,DX_DY_DZ;BOOLEAN COMP);
00200 BEGIN PRELOAD_WITH 3,7,11;
00300 SAFE OWN INTEGER ARRAY KEY[1:6];
00400 PRELOAD_WITH [6] 0.0; SAFE OWN REAL ARRAY DIR[1:6];
00500 INTEGER I,J;
00600 REAL MAX,R;
00700 LABEL OUT;
00800 REDUCE(DX_DY_DZ);
00900 IF DX_DY_DZ[1]=0.0 ∧
01000 DX_DY_DZ[2]=0.0 ∧
01100 DX_DY_DZ[3]=0.0 THEN BEGIN FOR I←1 STEP 1 UNTIL 6 DO DTH[I]←0.0;
01200 RETURN;
01300 END;
01400 UNDERFLOW(-1);
01500 ARRBLT(DIR[1],DX_DY_DZ[1],3);
01600 IF ¬COMP THEN BEGIN
01700 IF ABS(T[85+2])<ABS(T[85+6])
01800 THEN BEGIN KEY[4]←2;
01900 KEY[5]←IF ABS(T[85+6])<ABS(T[85+10]) THEN 6 ELSE 10
02000 END
02100 ELSE BEGIN KEY[4]←6;
02200 KEY[5]←IF ABS(T[85+2])<ABS(T[85+10]) THEN 2 ELSE 10
02300 END;
02400 KEY[6]←IF ABS(T[85+4])>ABS(T[85+0])
02500 THEN IF ABS(T[85+8])>ABS(T[85+4]) THEN 9 ELSE 5
02600 ELSE IF ABS(T[85+8])>ABS(T[85+0]) THEN 9 ELSE 1;
02700 MMOVE(U1[SQAR(6)],A[SQAR(6)]);
02800 FOR I←5 STEP -1 UNTIL 2 DO
02900 BEGIN MTIMES(U1[SQAR(I)],A[SQAR(I)],U1[SQAR(I+1)]);
03000 MTIMES(U1[SQAR(I+1)],Q[(IF I=2 THEN 17 ELSE 0)],U1[SQAR(I+1)])
03100 END;
03200 A[3]←A[7]←0.0;
03300 MMOVE(A[0],A[0]);
03400 MTIMES(U1[0],A[0],U1[SQAR(2)]);
03500 MTIMES(U1[SQAR(2)],Q[0],U1[SQAR(2)]);
03600 FOR I←2 STEP 1 UNTIL 6 DO MTIMES(U1[SQAR(I)],T[SQAR(I-1)],U1[SQAR(I)]);
03700 MTIMES(U1[0],Q[0],U1[0]);
03800 FOR I←1 STEP 1 UNTIL 6 DO
03900 FOR J←1 STEP 1 UNTIL 6 DO
04000 NR[J,I]←U1[SQAR(I)+KEY[J]];
04100 DECOMPOSE(6,NR,LU);
04200 END;
04300 SOLVE(6,LU,DIR,DTH);
04400 IMPROVE(6,NR,LU,DIR,DTH,DIGITS);
04500 MAX←0;
04600 FOR I←1 STEP 1 UNTIL 6 DO IF (R←ABS(DIR[I]))>MAX THEN MAX←R;
04700 FOR I←1 STEP 1 UNTIL 6 DO IF (R←ABS(DTH[I]))>MAX THEN
04800 BEGIN FOR J←I+1 STEP 1 UNTIL 6 DO IF ABS(DTH[I]+DTH[J])/R<0.5 THEN
04900 BEGIN DTH[I]←0;
05000 DTH[J]←0;
05100 GO TO OUT;
05200 END;
05300 END;
05400 OUT: FOR I←1 STEP 1 UNTIL 6 DO BEGIN
05500 DTH[I]←IF I=3 THEN DTH[I] ELSE RAD*DTH[I];
05600 NOT_LESS(DTH[I],@-5);
05700 END;
05800 UNDERFLOW(0);
05900 END;
06000
06100 INTEGER SIMPLE PROCEDURE LIMITS(SAFE REAL ARRAY JOINT);
06200 BEGIN INTEGER I;
06300 FOR I←1 STEP 1 UNTIL 5 DO
06400 IF (STOP[I,1]-JOINT[I])*(JOINT[I]-STOP[I,2])<0.0 THEN RETURN(I);
06500 RETURN (0);
06600 END;
06700
06800 SIMPLE PROCEDURE FAST_WRITE;
06900 IF FAST
07000 THEN BEGIN LABEL OK;
07100 START_CODE LABEL WRT;
07200 HRRZ 1,GOODIE;
07300 HRRZ 2,COEFF;
07400 MOVEM 2,(1);
07500 MOVE 3,BAND_NUMBER;
07600 MOVEI 4,10;
07700 WRT: UFBWRT 3,(1);
07800 SOJGE 4,WRT;
07900 MOVEI 2,'20;
08000 ADDM 2,2(1);
08100 JUMPGE 4,OK;
08200 END;
08300 USERERR(0,1,"TRAJECTORY WRITE ERROR");
08400 OK: END
08500 ELSE ARRYOUT('16,COEFF[0],'1000);
08600
08700 SIMPLE PROCEDURE FORCE(SAFE REAL ARRAY TQ,FV);
08800 BEGIN
08900 STRING S;
09000 INTEGER I,L;
09100 REAL XS,YS;
09200 SAFE OWN REAL ARRAY F,M,P[1:4];
09300 SAFE OWN REAL ARRAY INV[1:4,1:4];
09400 EXTERNAL SIMPLE PROCEDURE TRANSFORM(REAL ARRAY R;REFERENCE REAL A;REAL ARRAY V);
09500 EXTERNAL SIMPLE PROCEDURE INVERT(REFERENCE REAL R,A);
09600 SIMPLE PROCEDURE COLVECT(REAL ARRAY R;INTEGER L,I);
09700 BEGIN INTEGER K;
09800 FOR K←1 STEP 1 UNTIL 3 DO R[K]←A[L+(K-1)*4+(I-1)];
09900 R[4]←1.0;
10000 END;
10100
10200 PUSH_FORMAT(9,1);
10300 ARRBLT(F[1],FV[1],3);
10400 ARRBLT(M[1],FV[4],3);
10500 INVERT(INV[1,1],T[SQAR(6)]);
10600 F[4]←M[4]←0.0;
10700 TRANSFORM(F,INV[1,1],F);
10800 TRANSFORM(M,INV[1,1],M);
10900 A[3]←A[7]←0.0;
11000 FOR L←6 STEP -1 UNTIL 1 DO
11100 BEGIN
11200 F[4]←M[4]←0.0;
11300 TRANSFORM (F,A[SQAR(L)],F);
11400 COLVECT(P,SQAR(L),4);
11500 F[4]←1.0;
11600 CROSS(P,P,F);
11700 TRANSFORM(M,A[SQAR(L)],M);
11800 M[4]←1.0;
11900 PLUS(M,M,P);
12000 REDUCE(M);
12100 TQ[L]←IF L=3 THEN F[3] ELSE M[3];
12200 END;
12300 A[3]←A[3]+SHOLDER[1];
12400 A[7]←A[7]+SHOLDER[2];
12500 S←"JOINT TORQUES";
12600 FOR I←1 STEP 1 UNTIL 6 DO S←S&(CVF(TQ[I]));
12700 IF TYP_HAND THEN OUTSTR(S&CRLF&CRLF);
12800 POP_FORMAT;
12900 END;
13000
13100
13200 PRELOAD_WITH
13300 '400000200000,
13400 '200000040000,
13500 '100000010000,
13600 '40000002000,
13700 '20000000400,
13800 '10000000100;
13900 SAFE OWN INTEGER ARRAY GO_WORD[1:6];
14000 SIMPLE PROCEDURE BITS(REFERENCE INTEGER DATWD;REAL SIGN;REFERENCE INTEGER DAT);
14100 START_CODE
14200 MOVE 3,-3('17);
14300 MOVE 1,(3);
14400 MOVE 2,@-1('17);
14500 TDO 1,2;
14600 TRZ 1,(2);
14700 LSH 2,1;
14800 SKIPGE -2('17);
14900 TRO 1,(2);
15000 MOVEM 1,(3);
15100 END;
15200
15300 PROCEDURE FREE_JOINT(REFERENCE INTEGER DATWD);
15400 BEGIN INTEGER NF,I,J,K;
15500 REAL MAX;
15600 SAFE OWN INTEGER ARRAY ZF[1:6];
15700 SAFE OWN REAL ARRAY FV,TQ[1:6];
15800 NF←FREE_ARM[0,1];
15900 FOR I←1 STEP 1 UNTIL NF DO
16000 BEGIN
16100 ARRBLT(FV[1],FREE_ARM[I,1],6);
16200 FORCE(TQ,FV);
16300 MAX←0.0;
16400 FOR J←1 STEP 1 UNTIL 6 DO
16500 BEGIN LABEL L1;
16600 FOR K←1 STEP 1 UNTIL I-1 DO IF ZF[K]=J THEN GO TO L1;
16700 IF ABS(TQ[J]/F0[J])>MAX THEN
16800 BEGIN
16900 MAX←ABS(TQ[J]/F0[J]);
17000 ZF[I]←J;
17100 END;
17200 L1: END;
17300 BITS(DATWD,TQ[ZF[I]],GO_WORD[ZF[I]]);
17400 END;
17500 END;
17600
00100 SIMPLE PROCEDURE SCHEINMAN (SAFE REAL ARRAY RES,TH);
00200 BEGIN
00300 LABEL F2;
00400 REAL M,IXX,IYY,IZZ;
00500 SAFE OWN REAL ARRAY TQ[1:6];
00600 EXTERNAL REAL SIMPLE PROCEDURE INNER(REFERENCE REAL A,B);
00700 REAL Z; INTEGER I,DIFF,J,K,L,TJ;
00800 M←9.33*OBJECT_MASS+M6;
00900 Z←(M6*Z6)/M;
01000 IXX←I6XX+9.33*OBJECT_MASS*OBJECT_KXX↑2;
01100 IYY←I6YY+9.33*OBJECT_MASS*OBJECT_KYY↑2;
01200 IZZ←I6ZZ+9.33*OBJECT_MASS*OBJECT_KZZ↑2;
01300 I←SQAR(6);
01400 JMAT[I]←0.5*(-IXX+IYY+IZZ);
01500 JMAT[I+5]←0.5*(IXX-IYY+IZZ);
01600 JMAT[I+10]←0.5*(IXX+IYY-IZZ);
01700 JMAT[I+11]←JMAT[I+14]←M*Z;
01800 JMAT[I+15]←M;
01900 COMPLETA(1,TH);
02000 COMPLETA(2,TH);
02100 A[SQAR(3)+11]←TH[3];
02200 COMPLETA(4,TH);
02300 COMPLETA(5,TH);
02400 COMPLETA(6,TH);
02500 UNDERFLOW(-1);
02600 RES[1]←0;ARRBLT(RES[2],RES[1],11);
02700 MTIMES(U1[0],Q[0],A[0]);
02800 MTIMES(D[0],U1[0],JMAT[0]);
02900 RES[7]←RES[7]+TRACET(D[0],U1[0]);
03000 MMOVE(T[0],A[0]);
03100 for J ← 2 step 1 until 6 do begin
03200 TJ←SQAR(J);
03300 DIFF←IF J=3 THEN 17 ELSE 0;
03400 MTIMES (QA[0], Q[DIFF], A[TJ]);
03500 MTIMES (U1[TJ], T[SQAR(J-1)], QA[0]);
03600 for I← 1 step 1 until J-1 do MTIMES (U1[SQAR(I)],U1[SQAR(I)], A[TJ]);
03700 MTIMES(T[TJ],T[SQAR(J-1)],A[TJ]);
03800 F2: FOR I←1 STEP 1 UNTIL J DO RES[I]←RES[I]
03900 +0.107*INNER(U1[SQAR(I)+8],JMAT[TJ+12]);
04000 For K← 1 step 1 until J do begin
04100 MTIMES (D[0],U1[SQAR(K)],JMAT[TJ]);
04200 RES[K+6]←RES[K+6] + TRACET(D[0],U1[SQAR(K)]);
04300 END;
04400 END;
04500 T[88]←T[88]+SX;T[92]←T[92]+SY;
04600 FOR I←1 STEP 1 UNTIL 6 DO IF FORCE_ARM[I] THEN BEGIN
04700 FORCE(TQ,FORCE_ARM);
04800 FOR I←1 STEP 1 UNTIL 6 DO RES[I]←RES[I]+TQ[I];
04900 DONE;
05000 END;
05100 FOR I←1 STEP 1 UNTIL 12 DO NOT_LESS(RES[I],1.0@-10);
05200 I←'770000252502;
05300 IF NNUL THEN I←I+'1000000;
05400 FREE_JOINT(I);
05500 ARRBLT(RES[13],I,1);
05600 UNDERFLOW(0);
05700 END;
00100 SIMPLE PROCEDURE FLUSH(BOOLEAN FINISH;SAFE REAL ARRAY TS);
00200 BEGIN INTEGER I;
00300 IF ¬FRST_OPEN ∨ FINISH
00400 THEN BEGIN
00500 IFC WAVE THENC
00600 FOR I←1 STEP 1 UNTIL FREEL
00700 DO FOR J←1 STEP 1 UNTIL PTR3
00800 DO IF EQU(REF[J],LABELS[I])
00900 THEN BEGIN
01000 START_CODE
01100 MOVE 1,STACK;
01200 ADD 1,J;
01300 HRRE 1,-1(1);
01400 MOVEM 1,N END;
01500 N←PTRS[I]-J+N;
01600 REF[J]←NULL;
01700 IF N+J<1 ∨ N+J>PTR3+1
01800 THEN BEGIN
01900 OUTSTR(LABEL_LINE[I]&LABELS[I]&" OUT OF RANGE"&'15&'12);
02000 N←PTR3+1-J END;
02100 STACK[J]←(N LAND '777777) LOR (STACK[J] LAND '777000000);
02200 LABEL_LINE[I]←NULL END;
02300 FOR I←1 STEP 1 UNTIL PTR3
02400 DO IF LENGTH(REF[I])
02500 THEN BEGIN OUTSTR(CODE_LINE[I]&REF[I]&" UNDEFINED"&'15&'12);
02600 STACK[I]←(PTR3+1-I) LOR '102000000;
02700 REF[I]←NULL;
02800 LABEL_LINE[I]←NULL END;
02900 FREEL←0;
03000 FOR I←1 STEP 1 UNTIL PTR3 DO
03100 IF (J←(STACK[I] LAND '777000000))='102000000 ∨ J='16000000 THEN BEGIN
03200 START_CODE
03300 MOVE 1,STACK;
03400 ADD 1,I;
03500 HRRE 1,-1(1);
03600 MOVEM 1,N END;
03700 IF ¬N ∨ N+I<1 ∨ N+I>PTR3+1 THEN BEGIN
03800 OUTSTR(CODE_LINE[I]&"JUMP OUT OF RANGE"&CRLF);
03900 STACK[I]←(PTR3+1-I) LOR '102000000 END;
04000 END;
04100 FOR I←1 STEP 1 UNTIL MAC DO BBEG[I]←LLAB[I]←1;
04200 ENDC
04300 PTR3←PTR3+1;
04400 STACK[PTR3]←0;
04500 IF PTR1+PTR3+PTR4≥PTR2 THEN USERERR(0,1,"TRAJECTORY FILE TOO LONG");
04600 ARRBLT(COEFF[PTR1+1],STACK[1],PTR3);
04700 FOR I←1 STEP 1 UNTIL PTR3 DO IF RELOC[I] THEN COEFF[PTR1+I]←COEFF[PTR1+I]+PTR3+PTR1;
04800 ARRBLT(COEFF[PTR1+PTR3+1],DATA_STACK[1],PTR4);
04900 PTR1←-((PTR1+1) LOR (DURATION LSH 18));
05000 ARRBLT(COEFF[0],PTR1,1);
05100 COEFF['1000]←0;
05200 PTR1←PTR4←PTR3←0;
05300 RELOC[1]←0;ARRBLT(RELOC[2],RELOC[1],99);
05400 FAST_WRITE;
05500 END ELSE FRST_OPEN←FALSE;
05600 IF FINISH THEN BEGIN
05700 IF ¬FAST THEN RELEASE('16);
05800 RETURN END;
05900 COEFF[0]←0;ARRBLT(COEFF[1],COEFF[0],'1037);
06000 ARRTRAN(TH,TS);
06100 SCHEINMAN(DIAG,TH);
06200 ARRBLT(COEFF[PTR2←499],DIAG[1],13);
06300 END;
06400
00100 SIMPLE PROCEDURE REVOLVE(SAFE REAL ARRAY P,O; REAL TH);
00200 BEGIN
00300 SAFE OWN REAL ARRAY OP,A,T[1:4];
00400 UNIT(O,O);
00500 SCALE(OP,O,DOT(P,O));
00600 DIFFERENCE(A,P,OP);
00700 CROSS(T,O,A);
00800 SCALE(T,T,SIND(TH));
00900 SCALE(P,A,COSD(TH));
01000 PLUS(P,P,T);
01100 PLUS(P,P,OP);
01200 REDUCE(P);
01300 END;
00100 EXTERNAL SIMPLE PROCEDURE PACK(REFERENCE INTEGER PTR;INTEGER PERIOD;REFERENCE REAL BUF);
00200 SIMPLE PROCEDURE PACK_UP;BEGIN
00300 INTEGER P1,I,J,K;
00400 REAL R1,R2;
00500 SAFE OWN INTEGER ARRAY LOOPP,NXT,ALT[1:6];
00600 SAFE OWN REAL ARRAY BUF[0:4];
00700 PTR1←0;
00800 BUF[1]←0;BUF[2]←0;
00900 FOR JOINT←1 STEP 1 UNTIL 6 DO BEGIN
01000 BUF[0]←POSITION[JOINT,0];
01100 BUF[3]←KOE[JOINT,1];BUF[4]←KOE[JOINT,2];
01200 PACK(COEFF[PTR1←PTR1+4],PERIOD[JOINT,1],BUF[0]);
01300 NXT[JOINT]←PTR1 END;
01400 COEFF[PTR1←PTR1+1]←PTR2←486;
01500 FOR JOINT←1 STEP 1 UNTIL 6 DO BEGIN
01600 BUF[4]←0;
01700 K←IF LOOP THEN 7 ELSE NS[JOINT]-1;
01800 FOR I←2 STEP 1 UNTIL K DO BEGIN
01900 BUF[0]←POSITION[JOINT,I-1];
02000 FOR J←1 STEP 1 UNTIL 3 DO BUF[J]←KOE[JOINT,(3*I)-4+J];
02100 PACK(COEFF[PTR1←PTR1+4],PERIOD[JOINT,I],BUF[0]);
02200 COEFF[NXT[JOINT]]←(PTR1 LSH 18) LOR COEFF[NXT[JOINT]];
02300 NXT[JOINT]←PTR1;
02400 IF LOOP THEN BEGIN
02500 IF I=3 THEN LOOPP[JOINT]←PTR1;
02600 IF I=6 THEN ALT[JOINT]←PTR1 END;
02700 IF JOINT=6 THEN COEFF[PTR1←PTR1+1]←PTR2←PTR2-13 END;
02800 BUF[0]←POSITION[JOINT,I-1];
02900 BUF[1]←-4.0*(R1←KOE[JOINT,(3*I)-2])+3.0*(R2←KOE[JOINT,(3*I)-3]);
03000 BUF[2]← 6.0*R1-3.0*R2;
03100 BUF[3]←-4.0*R1 +R2;
03200 BUF[4]← R1;
03300 PACK(COEFF[PTR1←PTR1+4],PERIOD[JOINT,I],BUF[0]);
03400 COEFF[NXT[JOINT]]←(PTR1 LSH 18) LOR COEFF[NXT[JOINT]];
03500 BUF[4]←0;
03600 IF JOINT=6 THEN COEFF[PTR1←PTR1+1]←PTR2←PTR2-13;
03700 IF LOOP THEN BEGIN
03800 BUF[0]←POSITION[JOINT,6];
03900 FOR J←1 STEP 1 UNTIL 4 DO BUF[J]←KOE[JOINT,3*I-2+J];
04000 PACK(COEFF[PTR1←PTR1+4],PERIOD[JOINT,3],BUF[0]);
04100 NXT[JOINT]←PTR1;
04200 COEFF[P1←ALT[JOINT]]←(PTR1 LSH 27) LOR COEFF[P1];
04300 IF JOINT=6 THEN COEFF[P1+1]←'100000000000 LOR (LOOP LSH 18) LOR COEFF[P1+1];
04400 IF JOINT=6 THEN COEFF[PTR1←PTR1+1]←499;
04500 BUF[0]←POSITION[JOINT,0];
04600 FOR J←1 STEP 1 UNTIL 4 DO BUF[J]←KOE[JOINT,3*I+2+J];
04700 PACK(COEFF[PTR1←PTR1+4],PERIOD[JOINT,3],BUF[0]);
04800 COEFF[NXT[JOINT]]←(PTR1 LSH 18) LOR COEFF[NXT[JOINT]];
04900 COEFF[PTR1]←(LOOPP[JOINT] LSH 18) LOR COEFF[PTR1];
05000 IF JOINT=6 THEN COEFF[PTR1←PTR1+1]←473 END END;
05100 STACK[PTR3←PTR3+1]←'777777000000;
05200 END;
05300
00100 IFC WAVE THENC
00200 SIMPLE PROCEDURE STEP_ARM(INTEGER JOINT;REAL DTH;INTEGER TIME);
00300 BEGIN
00400 INTEGER I;
00500 IF ARM_EXECUTE THEN RETURN;
00600 FLUSH(0,LAST_ARM);
00700 FOR I←1 STEP 1 UNTIL 6 DO
00800 BEGIN
00900 POSITION[I,0]←LAST_ARM[I];
01000 NS[I]←2;
01100 PERIOD[I,1]←10;
01200 PERIOD[I,2]←IF TIME>60 THEN TIME-10 ELSE 50;
01300 IF I=JOINT THEN POSITION[I,1]←POSITION[I,2]←LAST_ARM[I]←LAST_ARM[I]+DTH
01400 ELSE POSITION[I,1]←POSITION[I,2]←LAST_ARM[I];
01500 END;
01600 COEFF[511]←GO_WORD[JOINT]+2;
01700 ARRBLT(COEFF[PTR2←PTR2-13],COEFF[PTR2+13],13);
01800 ARRBLT(COEFF[PTR2←PTR2-13],COEFF[PTR2+13],13);
01900 KOE[1,1]←0;
02000 ARRBLT(KOE[1,2],KOE[1,1],6*((MAX_SEG*3)+6)-1);
02100 PACK_UP;
02200 DURATION←10;
02300 END;
02400
02500 ENDC
02600
02700 PRELOAD_WITH 0.5, 0.5, 0.25, 1.0, 1.0, 2.5;
02800 SAFE REAL ARRAY OSHOOT[1:6];
02900
03000 BOOLEAN SIMPLE PROCEDURE OVERSHOOT(REAL A3,A4,DELTA,OFF;REFERENCE REAL T,D);
03100 RETURN(A4 ∧ 0+OFF< (T←-(3*A3)/(4*A4)) <1+OFF
03200 ∧ ABS(D←(IF(D←-A3*T↑3/4)*DELTA<0 THEN DELTA+D ELSE D))>OSHOOT[JOINT]);
03300
00100 SIMPLE PROCEDURE POLY(INTEGER N);BEGIN
00200 SAFE OWN REAL ARRAY A,LU[1:(3*MAX_SEG)+6,1:(3*MAX_SEG)+6];
00300 SAFE OWN REAL ARRAY B[1:(3*MAX_SEG)+6];
00400 INTEGER I,J,K,P1,N3;
00500 REAL T,T2;
00600 SIMPLE PROCEDURE THREE(INTEGER I,J;REAL T);
00700 BEGIN
00800 FOR K←J STEP 1 UNTIL J+2 DO A[I+2,K]←1.0;
00900 A[I,J]←-T;
01000 A[I+1,J+1]←-(A[I+4,J+1]←2.0*(T2←T*T));
01100 A[I+4,J+2]←6.0*T2;
01200 A[I+3,J+2]←3.0*T;
01300 A[I+3,J+1]←2.0*T;
01400 A[I+3,J]←T;
01500 END;
01600
01700 SIMPLE PROCEDURE FOUR(INTEGER I,J;REAL T);
01800 BEGIN
01900 FOR K←J STEP 1 UNTIL J+3 DO A[I+2,K]←1.0;
02000 A[I,J]←-T;
02100 A[I+1,J+1]←-(A[I+4,J+1]←2.0*(T2←T*T));
02200 A[I+4,J+2]←6.0*T2;
02300 A[I+4,J+3]←12.0*T2;
02400 A[I+3,J+3]←4.0*T;
02500 A[I+3,J+2]←3.0*T;
02600 A[I+3,J+1]←2.0*T;
02700 A[I+3,J]←T;
02800 END;
02900
03000 UNDERFLOW(-1);
03100 N3←N*3;
03200 T←1.0/TIM[1];
03300 T2←T*T;
03400 A[1,1]←A[1,2]←1.0;
03500 A[2,1]←3.0*T;
03600 A[2,2]←4.0*T;
03700 A[3,1]←6.0*T2;
03800 A[3,2]←12.0*T2;
03900 FOR J←1 STEP 1 UNTIL N-2 DO
04000 THREE((3*J)-1,(3*J),1.0/TIM[J+1]);
04100 A[N3-4,N3-3]←-3.0*(T←1.0/TIM[N]);
04200 A[N3-4,N3-2]←4.0*T;
04300 A[N3-3,N3-3]←6.0*(T2←T*T);
04400 A[N3-3,N3-2]←-12.0*T2;
04500 A[N3-2,N3-3]←1.0;
04600 A[N3-2,N3-2]←-1.0;
04700 FOR J←1 STEP 1 UNTIL N DO
04800 B[(J*3)-2]←POS[J]-POS[J-1];
04900 P1←N3-2;
05000 IF LOOP THEN BEGIN
05100 FOUR(N3-1,N3-1,(T←1/TIM[N-2]));
05200 FOUR(N3+2,N3+3,T);
05300 FOR J←6,7,8 DO BEGIN
05400 A[N3+5,J]←A[5,J];
05500 A[N3+6,J]←A[6,J] END;
05600 FOR J←N3-9 STEP 1 UNTIL N3-7 DO BEGIN
05700 A[N3-1,J]←A[N3-7,J];
05800 A[N3,J]←A[N3-6,J] END;
05900 B[N3+1]←B[N3-2]+B[N3-5];
06000 B[N3+4]←B[1]+B[4];
06100 P1←P1+8;
06200 END;
06300 DECOMPOSE(P1,A,LU);
06400 SOLVE(P1,LU,B,SOLN);
06500 IMPROVE(P1,A,LU,B,SOLN,T2);
06600 UNDERFLOW(0);
06700 END;
06800 INTEGER SIMPLE PROCEDURE QUADROOT(REAL A0,A1,A2,A3;REFERENCE REAL ANG,T);BEGIN
06900 REAL DISC;
07000 INTEGER I;
07100 IF A3 ∧ (DISC←(A2/(3*A3))↑2-A1/(3*A3))≥0 THEN DISC←SQRT(DISC) ELSE RETURN(0);
07200 T←DISC-A2/(3*A3);
07300 FOR I←0,1 DO BEGIN
07400 IF 0< T <1.0 THEN
07500 BEGIN
07600 ANG←(((A3*T)+A2)*T+A1)*T+A0;
07700 IF ANG>STOP[JOINT,2] THEN BEGIN ANG←STOP[JOINT,2];RETURN(1) END;
07800 IF ANG<STOP[JOINT,1] THEN BEGIN ANG←STOP[JOINT,1];RETURN(1) END;
07900 END;
08000 T←-(T+2*A2/3*A3);
08100 END;
08200 RETURN(0);
08300 END;
08400
00100 INTEGER SIMPLE PROCEDURE KISEKI;BEGIN
00200 INTEGER N,I,J,OVER;
00300 REAL ANG,T;
00400 SIMPLE PROCEDURE BUMP_UP(INTEGER I;REAL ANG,T);
00500 BEGIN INTEGER J;
00600 OVER←OVER+1;
00700 FOR J←MAX_SEG STEP -1 UNTIL I+1 DO BEGIN
00800 POSITION[JOINT,J]←POSITION[JOINT,J-1];
00900 PERIOD[JOINT,J]←PERIOD[JOINT,J-1] END;
01000 POSITION[JOINT,I]←ANG;
01100 PERIOD[JOINT,I]←T*PERIOD[JOINT,I];
01200 IF PERIOD[JOINT,I]<1 THEN PERIOD[JOINT,I]←1;
01300 PERIOD[JOINT,I+1]←PERIOD[JOINT,I+1]-PERIOD[JOINT,I] END;
01400 FOR JOINT←1 STEP 1 UNTIL 6 DO BEGIN
01500 ARRBLT(POS[0],POSITION[JOINT,0],NS[JOINT]+1);
01600 ARRBLT(TIM[1],PERIOD[JOINT,1],NS[JOINT]);
01700 POLY(N←NS[JOINT]);
01800 OVER←0;
01900 IF OVERSHOOT(SOLN[1],SOLN[2],POS[1]-POS[0],0,T,ANG)
02000 THEN BUMP_UP(2,POS[1]+ANG,(1.0-T)*TIM[1]/TIM[2]);
02100 IF OVERSHOOT(SOLN[J←(N-1)*3],SOLN[J+1],POS[N-1]-POS[N],-1.0,T,ANG)
02200 THEN BUMP_UP(N+OVER-1,POS[N-1]+ANG,1.0-(1.0+T)*TIM[N]/TIM[N-1]);
02300 IF OVER THEN BEGIN
02400 NS[JOINT]←N+OVER;
02500 ARRBLT(POS[0],POSITION[JOINT,0],NS[JOINT]+1);
02600 ARRBLT(TIM[1],PERIOD[JOINT,1],NS[JOINT]);
02700 POLY(NS[JOINT])END;
02800 ARRBLT(KOE[JOINT,1],SOLN[1],(NS[JOINT]*3)+4) END;
02900 FOR J←1 STEP 1 UNTIL 6 DO TH[J]←POSITION[J,1];
03000 SCHEINMAN(DIAG,TH);
03100 ARRBLT(COEFF[PTR2←PTR2-13],DIAG[1],13);
03200 IF NS[6]=5 ∨ (NS[6]=4 ∧ PERIOD[6,2]≤PERIOD[6,3]) THEN ARRBLT(COEFF[PTR2←PTR2-13],COEFF[PTR2+13],13);
03300 FOR J←1 STEP 1 UNTIL 6 DO TH[J]←POSITION[J,NS[J]-1];
03400 SCHEINMAN(DIAG,TH);
03500 ARRBLT(COEFF[PTR2←PTR2-13],DIAG[1],13);
03600 IF NS[6]=5 ∨ (NS[6]=4 ∧ PERIOD[6,2]>PERIOD[6,3]) THEN ARRBLT(COEFF[PTR2←PTR2-13],COEFF[PTR2+13],13);
03700 FOR J←1 STEP 1 UNTIL 6 DO TH[J]←POSITION[J,NS[J]];
03800 SCHEINMAN(DIAG,TH);
03900 ARRBLT(COEFF[PTR2←PTR2-13],DIAG[1],13);
04000 COMMENT THIS IS WHERE THE FORCE OPTIMIZATION GOES;
04100 FOR JOINT←2 STEP 1 UNTIL 5 DO BEGIN
04200 OVER←0;
04300 FOR I←NS[JOINT]-1 STEP -1 UNTIL 2 DO
04400 IF QUADROOT(POSITION[JOINT,I-1],KOE[JOINT,(I*3)-3],
04500 KOE[JOINT,(I*3)-2],KOE[JOINT,(I*3)-1],ANG,T)
04600 THEN BUMP_UP(I,ANG,T);
04700 NS[JOINT]←NS[JOINT]+OVER;
04800 IF OVER THEN BEGIN
04900 ARRBLT(POS[0],POSITION[JOINT,0],NS[JOINT]+1);
05000 ARRBLT(TIM[1],PERIOD[JOINT,1],NS[JOINT]);
05100 POLY(NS[JOINT]);
05200 ARRBLT(KOE[JOINT,1],SOLN[1],(NS[JOINT]*3)+4) END END;
05300 COMMENT THIS IS WHERE THE COLLISION AVOIDER GOES;
05400 PACK_UP;
05500 RETURN(0) END;
05600
05700 PROCEDURE RESET_CONO;
05800 BEGIN
05900 FREE_ARM[0,1]←FORCE_ARM[1]←OBJECT_MASS←OBJECT_KXX←OBJECT_KYY←OBJECT_KZZ←0;
06000 NNUL←0;
06100 ARRBLT(FORCE_ARM[2],FORCE_ARM[1],5);
06200 END;
06300
06400
00100 MP SIMPLE PROCEDURE TRAJECTORY(REAL ARRAY T0,TF);BEGIN"MARK1"
00200 INTEGER MT,I,J,N;
00300 REAL INT,R;
00400 SAFE OWN REAL ARRAY AI,AF[1:6];
00500 INTEGER TR1,TR2;
00600 SIMPLE PROCEDURE LIFTOFF(INTEGER SPEED;REFERENCE REAL F;SAFE REAL ARRAY TH,DTH,DIR;REFERENCE INTEGER N,T1);
00700 BEGIN INTEGER I,J;
00800 LABEL L1,L2;
00900 SAFE OWN REAL ARRAY TT[1:6];
01000 F←1.0;
01100 IF LIMITS(TH) THEN BEGIN
01200 FOR I←1 STEP 1 UNTIL 5 DO DTH[I]←
01300 IF TH[I]≥STOP[I,2] THEN STOP[I,2]-TH[I]
01400 ELSE IF TH[I]≤STOP[I,1] THEN STOP[I,1]-TH[I]
01500 ELSE 0;
01600 GO TO L2;
01700 END;
01800 HANDPOS(TH);
01900 INCREMENT(DTH,DIR,FALSE);
02000 L1: FOR I←1 STEP 1 UNTIL 6 DO TT[I]←TH[I]+2*DTH[I];
02100 I←LIMITS(TT);
02200 IF I THEN
02300 BEGIN FOR J←1,2 DO
02400 IF (TT[I]-STOP[I,J])*(STOP[I,J]-TH[I])≥0
02500 THEN
02600 BEGIN F←0.9*(STOP[I,J]-TH[I])/(TT[I]-TH[I]);
02700 DTH[I]←DTH[I]*F;
02800 DONE;
02900 END;
03000 GO TO L1;
03100 END;
03200 L2: N←IF T1>5 THEN T1 ELSE 5;
03300 FOR I←1 STEP 1 UNTIL 6 DO IF(J←SPEED*SQRT(ABS(DTH[I])/MA[I]))>N THEN N←J;
03400 END;
03500
03600 WHILE TF[6]-T0[6]>180.0 DO TF[6]←TF[6]-360.0;
03700 WHILE TF[6]-T0[6]≤-180.0 DO TF[6]←TF[6]+360.0;
03800 FLUSH(0,T0);
03900 LIFTOFF(3,F1_ARM,TH,AI,DEPART_ARM,TR1,T1_ARM);
04000 LIFTOFF(6,F2_ARM,TF,AF,ARRIVE_ARM,TR2,T2_ARM);
04100 IF DEB_HAND THEN OUTSTR("TIMES");
04200 N←TR1+TR2;
04300 FOR I←1 STEP 1 UNTIL 6 DO BEGIN
04400 IF (J←2*SQRT(ABS(TH[I]-TF[I])/MA[I]))>N THEN N←J;
04500 IF DEB_HAND THEN OUTSTR(CVS(J)) END;
04600 IF DEB_HAND THEN OUTSTR(CRLF);
04700 FOR I←1 STEP 1 UNTIL 6 DO BEGIN
04800 POSITION[I,0]←T0[I];
04900 POSITION[I,1]←T0[I]+AI[I];
05000 POSITION[I,2]←TF[I]+AF[I];
05100 COMMENT POSITION[I,2]←IF (INT←AI[I]/TR1+AF[I]/TR2) ∧ MT←ABS(INT)>1.0@-4
05200 ∧ 0 < (MT←(POSITION[I,3]-POSITION[I,1]+AF[I]*N/TR2)/INT)<N
05300 THEN AI[I]*MT/TR1+POSITION[I,1]
05400 ELSE (POSITION[I,1]+POSITION[I,3]+INT*N/2)/2;
05500 POSITION[I,3]←TF[I];
05600 NS[I]←3;
05700 PERIOD[I,1]←TR1;
05800 COMMENT PERIOD[I,2]←IF 0<MT<N THEN MT ELSE N/2;
05900 PERIOD[I,2]←N;
06000 PERIOD[I,3]←TR2 END;
06100 LOOP←0;
06200 IF KISEKI THEN USERERR(0,1,"OVERSHOOT");
06300 DURATION←N;
06400 ARRTRAN(LAST_ARM,TF);
06500 ARM_TIME←ARM_TIME+N+TR1+TR2;
06600 FOR I←1 STEP 1 UNTIL 3 DO ARRIVE_ARM[I]←DEPART_ARM[I]←IF I=3 THEN 3.0 ELSE 0.0;
06700 ARRIVE_ARM[4]←DEPART_ARM[4]←1.0;
06800 T1_ARM←T2_ARM←20;
06900 RESET_CONO;
07000 END"MARK1";
00100 MP SIMPLE PROCEDURE DRAW_ARM(REAL ARRAY PROFILE;INTEGER STAT);
00200 BEGIN
00300 SAFE OWN REAL ARRAY TRANS[1:4,1:4];
00400 SAFE OWN REAL ARRAY FAS[0:6,1:6];
00500 SAFE OWN REAL ARRAY ROTS,ROTA,RS,IP,RV,CV,FVV,FV,VT,DIFF[1:4];
00600 REAL AR,RR,THP4,R,MAX;
00700 INTEGER TIME,NF,I,J,K,NT;
00800 EXTERNAL SIMPLE PROCEDURE MOVEV(REAL ARRAY R;REFERENCE REAL A);
00900 IF ARM_EXECUTE THEN BEGIN STAT←4;ARM_EXECUTE←FALSE;RETURN END;
01000 ARRTRAN(TRANS,LAST_TRANS);
01100 CVV(IP,LAST_TRANS,4);
01200 ARRTRAN(FAS,FREE_ARM);
01300 MOVEV(DIFF,PROFILE[1,1]);
01400 MOVEV(RV,PROFILE[2,1]);
01500 IF(AR←IF MAGNITUDE(RV) THEN PROFILE[3,1] ELSE 0) THEN UNIT(RV,RV);
01600 MOVEV(ROTA,PROFILE[5,1]);
01700 IF(RR←IF MAGNITUDE(ROTA) THEN PROFILE[3,2] ELSE 0)
01800 THEN BEGIN UNIT(ROTA,ROTA);
01900 MOVEV(ROTS,PROFILE[4,1]);
02000 DIFFERENCE(IP,IP,ROTS) END;
02100 FVV[1]←FV[1]←FORCE_ARM[1];
02200 FVV[2]←FV[2]←FORCE_ARM[2];
02300 FVV[3]←FV[3]←FORCE_ARM[3];
02400 FVV[4]←FV[4]←1.0;
02500 FLUSH(0,LAST_ARM);
02600 FRST_OPEN←TRUE;
02700 R←0;
02800 IF PROFILE[0,3] THEN LOOP←PROFILE[0,3]-1;
02900 IF LOOP THEN NT←8 ELSE BEGIN
03000 NT←4;
03100 R←MAGNITUDE(DIFF);
03200 IF R>15.0 THEN NT←(R/5)+0.5;
03300 NT←IF (MAX←AR/60.0)>NT THEN MAX ELSE NT;
03400 NT←IF (MAX←RR/60.0)>NT THEN MAX ELSE NT END;
03500 IF TYP_HAND THEN OUTSTR(CVS(NT)&" PART TRAJECTORY"&CRLF);
03600 IF NT>8 THEN BEGIN STAT←3;RETURN END;
03700 FOR J←1 STEP 1 UNTIL 6 DO POSITION[J,0]←LAST_ARM[J];
03800 TH[6]←LAST_ARM[6];
03900 FOR I←1 STEP 1 UNTIL NT DO
04000 BEGIN
04100 R←IF I=1
04200 THEN 1/(3*(NT-2))
04300 ELSE (I-1)/(NT-2);
04400 R←IF I=NT-1
04500 THEN R-1/(3*(NT-2))
04600 ELSE IF I=NT
04700 THEN 1.0
04800 ELSE R;
04900 SCALE(VT,DIFF,R);
05000 PLUS(VT,VT,IP);
05100 IF RR THEN BEGIN ROTATE(RS,ROTS,ROTA,R*RR);
05200 PLUS(VT,VT,RS)END;
05300 REDUCE(VT);
05400 CVC(TRANS,4,VT);
05500 IF AR THEN FOR J←1 STEP 1 UNTIL 3 DO BEGIN
05600 CVV(CV,LAST_TRANS,J);
05700 REVOLVE(CV,RV,R*AR);
05800 CVC(TRANS,J,CV) END;
05900 THP4←TH[4];
06000 ARM_SOLVE(TRANS,TH,STAT);
06100 IF ¬STAT THEN BEGIN STAT←1;RETURN END;
06200 IF ABS(THP4-TH[4])>90.0 THEN BEGIN STAT←2;RETURN END;
06300 FOR J←1 STEP 1 UNTIL 6 DO POSITION[J,I]←TH[J];
06400 IF RR THEN BEGIN
06500 FOR K←1 STEP 1 UNTIL FAS[0,1] DO
06600 BEGIN
06700 MOVEV(VT,FAS[K,1]);
06800 VT[4]←1.0;
06900 IF MAGNITUDE(VT)>0.0 THEN
07000 BEGIN
07100 REVOLVE(VT,ROTA,R*RR);
07200 REDUCE(VT);
07300 FOR J←1 STEP 1 UNTIL 3 DO FREE_ARM[K,J]←VT[J] END END;
07400 MOVEV(FV,FVV[1]);
07500 REVOLVE(FV,ROTA,R*RR);
07600 REDUCE(FV);
07700 ARRBLT(FORCE_ARM[1],FV[1],3) END;
07800 SCHEINMAN(DIAG,TH);
07900 ARRBLT(COEFF[PTR2←PTR2-13],DIAG[1],13);
08000 END;
08100 FOR I←1 STEP 1 UNTIL 6 DO IF(J←2*SQRT(ABS(TH[I]-LAST_ARM[I])/MA[I]))>PROFILE[0,2] THEN PROFILE[0,2]←J;
08200 TIME←(PROFILE[0,2]/NT)+0.5;
08300 FOR JOINT←1 STEP 1 UNTIL 6 DO BEGIN
08400 NS[JOINT]←NT;
08500 FOR J←1 STEP 1 UNTIL NT DO PERIOD[JOINT,J]←TIME;
08600 ARRBLT(POS[0],POSITION[JOINT,0],NS[JOINT]+1);
08700 ARRBLT(TIM[1],PERIOD[JOINT,1],NS[JOINT]);
08800 POLY(NS[JOINT]);
08900 ARRBLT(KOE[JOINT,1],SOLN[1],(NS[JOINT]*3)+6) END;
09000 PACK_UP;
09100 DURATION←TIME;
09200 ARRTRAN(LAST_ARM,TH);
09300 ARRTRAN(LAST_TRANS,TRANS);
09400 ARM_TIME←ARM_TIME+TIME*((NT-2)*LOOP+2);
09500 STAT←0;
09600 RESET_CONO;
09700 FRST_OPEN←FALSE;
09800 END;
00100 BOOLEAN SIMPLE PROCEDURE IS_NOT_CLEAR(SAFE REAL ARRAY TRANS,J);
00200 BEGIN SAFE OWN REAL ARRAY IP[1:4];
00300 INTEGER FLAG;
00400 REAL R;
00500 BOOLEAN SIMPLE PROCEDURE NOT_CLEAR(REFERENCE REAL J;SAFE REAL ARRAY V);
00600 BEGIN SAFE OWN REAL ARRAY VT[1:4];
00700 EXTERNAL SIMPLE PROCEDURE TRANSFORM(REAL ARRAY V;REFERENCE REAL T;REAL ARRAY F);
00800 TRANSFORM(VT,J,V);
00900 IF VT[3]<0.375 THEN RETURN(TRUE);
01000 IF (VT[1]-SHOLDER[1])↑2+(VT[2]-SHOLDER[2])↑2<25.0 THEN RETURN(TRUE) ELSE RETURN(FALSE);
01100 END;
01200 IP[1]←IP[3]←0.0;IP[4]←1.0;
01300 FOR R←-2.0,2.0 DO BEGIN IP[2]←R;IF NOT_CLEAR(TRANS[1,1],IP)THEN RETURN(TRUE);END;
01400 J[6]←LAST_ARM[6];
01500 ARM_SOLVE(TRANS,J,FLAG);
01600 IF ¬FLAG THEN RETURN(TRUE);
01700 HANDPOS(J);
01800 IP[2]←0.0;
01900 FOR R←3.0,-3.0 DO BEGIN IP[3]←R;IF NOT_CLEAR(T[SQAR(4)],IP) THEN RETURN(TRUE);END;
02000 IP[3]←-38.0;
02100 IF NOT_CLEAR(T[SQAR(3)],IP) THEN RETURN(TRUE) ELSE RETURN(FALSE);
02200 END;
02300
00100 MP SIMPLE PROCEDURE START_TRAJECTORY(STRING FILE;INTEGER START);
00200 BEGIN
00300 STRING S;
00400 LABEL FIND,NONE,FOUND;
00500 REAL R;
00600 FRST_OPEN←TRUE;
00700 ARM_TIME←0;
00800 IF ¬START THEN ARRTRAN(LAST_ARM,ARM_VECTOR);
00900 WHILE LAST_ARM[6]>180.0 DO LAST_ARM[6]←LAST_ARM[6]-360.0;
01000 WHILE LAST_ARM[6]≤180.0 DO LAST_ARM[6]←LAST_ARM[6]+360.0;
01100 HANDPOS(LAST_ARM);
01200 ARRBLT(LAST_TRANS[1,1],T[SQAR(6)],16);
01300 IF FAST THEN BEGIN
01400 FIND: IF NEXT_BAND>'37 THEN GO TO NONE;
01500 FOR BAND←0 STEP 1 UNTIL NEXT_BAND-1 DO IF EQU(FILE,BANDS[BAND])THEN GO TO FOUND;
01600 TRACK[BAND]←CALL('100000000000 LOR BAND,"UFBGET");
01700 IF ¬_SKIP_ THEN GO TO NONE;
01800 NEXT_BAND←BAND+1;
01900 BANDS[BAND]←FILE;
02000 FOUND: GOODIE[3]←0;
02100 BAND_NUMBER←TRACK[BAND];
02200 END ELSE NONE:BEGIN
02300 FAST←FALSE;
02400 OPEN('16,"DSK",'17,0,0,120,BREAK,EOF);
02500 ENTER('16,FILE&".TRJ",BREAK);
02600 END;
02700 COEFF[0]←0; ARRBLT(COEFF[1],COEFF[0],'1037);
02800 RELOC[1]←0;ARRBLT(RELOC[2],RELOC[1],99);
02900 PTR1←PTR4←PTR3←0;
03000 PTR2←512;
03100 END;
03200
03300 MP SIMPLE PROCEDURE CLOSE_TRAJECTORY;
03400 FLUSH(1,TH);
03500
03600 MP SIMPLE PROCEDURE ARM_POSITION;
03700 BEGIN
03800 ARMPOS;
03900 UPDATE_SEG;
04000 END;
04100
00100 MP SIMPLE PROCEDURE OPEN_HAND(REAL OPENING);
00200 IF ARM_EXECUTE
00300 THEN BEGIN ARM_MESSAGE[1]←'1000000 LOR (OPENING LSH -18);
00400 HANDFN;
00500 ARM_EXECUTE←FALSE;
00600 UPDATE_SEG END
00700 ELSE STACK[PTR3←PTR3+1]←'1000000 LOR (OPENING LSH -18);
00800
00900 MP SIMPLE PROCEDURE WOBBLE_HAND(REAL WOBBLEING);
01000 IF ARM_EXECUTE
01100 THEN BEGIN ARM_MESSAGE[1]←'14000000 LOR (WOBBLEING LSH -18);
01200 HANDFN;
01300 ARM_EXECUTE←FALSE;
01400 UPDATE_SEG END
01500 ELSE STACK[PTR3←PTR3+1]←'14000000 LOR (WOBBLEING LSH -18);
01600
01700 MP SIMPLE PROCEDURE PLACE_ARM;
01800 BEGIN SAFE OWN REAL ARRAY DIR[1:4],DTH[1:6];
01900 INTEGER I;
02000 FOR I←1 STEP 1 UNTIL 3 DO DIR[I]←IF I=3 THEN -0.03 ELSE 0;
02100 DIR[4]←1.0;
02200 INCREMENT(DTH,DIR,FALSE);
02300 IF ARM_EXECUTE
02400 THEN BEGIN ARM_MESSAGE[7]←'4000000;
02500 ARRBLT(ARM_MESSAGE[1],DTH[1],6);
02600 ARMFN(6);
02700 ARM_EXECUTE←FALSE;
02800 UPDATE_SEG END
02900 ELSE BEGIN STACK[PTR3←PTR3+1]←'4000000+(PTR4←PTR4+1);
03000 RELOC[PTR3]←-1;
03100 ARRBLT(DATA_STACK[PTR4],DTH[1],6);
03200 PTR4←PTR4+5 END;
03300 END;
03400
03500 MP SIMPLE PROCEDURE SEARCH_ARM(REAL R);
03600 BEGIN SAFE OWN REAL ARRAY DIR[1:4],DTH[1:6];
03700 INTEGER I;
03800 FOR I←1 STEP 1 UNTIL 3 DO DIR[I]←IF I=1 THEN R ELSE 0;
03900 DIR[4]←1.0;
04000 INCREMENT(DTH,DIR,FALSE);
04100 IF ARM_EXECUTE THEN BEGIN ARM_EXECUTE←FALSE; RETURN END;
04200 STACK[PTR3←PTR3+1]←'15000000+(PTR4←PTR4+1);
04300 RELOC[PTR3]←-1;
04400 ARRBLT(DATA_STACK[PTR4],DTH[1],6);
04500 PTR4←PTR4+6;
04600 DIR[1]↔DIR[2];
04700 INCREMENT(DTH,DIR,TRUE);
04800 ARRBLT(DATA_STACK[PTR4],DTH[1],6);
04900 PTR4←PTR4+5;
05000 END;
05100
05200 MP SIMPLE PROCEDURE PARK_ARM;
05300 BEGIN SAFE OWN REAL ARRAY DTH[1:6],EXF[1:13];
05400 INTEGER K,TIME;
05500 IF ¬ARM_EXECUTE THEN
05600 BEGIN
05700 IF TYP_HAND THEN PMAT("TRAJECTORY FROM",LAST_TRANS);
05800 HANDPOS(V0);
05900 ARRBLT(LAST_TRANS[1,1],T[SQAR(6)],16);
06000 ARRIVE_ARM[3]←0.0;
06100 IF TYP_HAND THEN PMAT("TRAJECTORY TO",LAST_TRANS);
06200 TRAJECTORY(LAST_ARM,V0);
06300 END ELSE BEGIN
06400 ARM_EXECUTE←FALSE;
06500 FOR I←1 STEP 1 UNTIL 6 DO DTH[I]←V0[I]-ARM_VECTOR[I];
06600 WHILE DTH[6]>180.0 DO DTH[6]←DTH[6]-360.0;
06700 WHILE DTH[6]≤-180.0 DO DTH[6]←DTH[6]+360.0;
06800 ARM_MESSAGE[21]←'5000000;
06900 ARRBLT(ARM_MESSAGE[1],DTH[1],6);
07000 SCHEINMAN(EXF,V0);
07100 ARRBLT(ARM_MESSAGE[7],EXF[1],13);
07200 TIME←20;
07300 RESET_CONO;
07400 FOR I←1 STEP 1 UNTIL 6 DO IF(K←6*SQRT(ABS(DTH[I])/MA[I]))>TIME THEN TIME←K;
07500 ARM_MESSAGE[20]←TIME;
07600 ARMFN(20);
07700 UPDATE_SEG END
07800 END;
07900
08000 MP SIMPLE PROCEDURE WAIT_ARM;
08100 IF ¬ARM_EXECUTE THEN STACK[PTR3←PTR3+1]←'3000000 ELSE ARM_EXECUTE←FALSE;
08200
08300 MP SIMPLE PROCEDURE CLOSE_HAND(REAL DIST);
08400 IF ARM_EXECUTE
08500 THEN BEGIN ARM_MESSAGE[1]←'2000000 LOR (DIST LSH -18);
08600 HANDFN;
08700 ARM_EXECUTE←FALSE;
08800 UPDATE_SEG END
08900 ELSE STACK[PTR3←PTR3+1]←'2000000 LOR (DIST LSH -18);
09000
00100 MP SIMPLE PROCEDURE CHANGE_ARM(REAL ARRAY DIR;REAL DIST;
00200 REAL ARRAY AXIS;REAL DEG;INTEGER TIME;REFERENCE INTEGER FLAG);
00300 BEGIN INTEGER J,N;
00400 SAFE OWN REAL ARRAY AV,TQ,DTH[1:6];
00500 SAFE OWN REAL ARRAY EXF[1:13];
00600 SAFE OWN REAL ARRAY TT[1:4,1:4];
00700 SAFE OWN REAL ARRAY VT[1:4];
00800 IF ARM_EXECUTE
00900 THEN BEGIN ARRBLT(TT[1,1],T[85],16);
01000 ARRBLT(AV[1],ARM_VECTOR[1],6) END
01100 ELSE BEGIN ARRTRAN(TT,LAST_TRANS);
01200 ARRTRAN(AV,LAST_ARM)END;
01300 SCALE(VT,DIR,DIST);
01400 REDUCE(VT);
01500 FOR J←1 STEP 1 UNTIL 3 DO TT[J,4]←TT[J,4]+VT[J];
01600 IF DEG ∧ MAGNITUDE(AXIS) THEN BEGIN
01700 UNIT(AXIS,AXIS);
01800 FOR I←1 STEP 1 UNTIL 3 DO BEGIN
01900 CVV(VT,TT,I);
02000 REVOLVE(VT,AXIS,DEG);
02100 CVC(TT,I,VT) END;
02200 END;
02300 DTH[6]←AV[6];
02400 ARM_SOLVE(TT,DTH,FLAG);
02500 IF ¬FLAG THEN BEGIN ARM_EXECUTE←FALSE;RETURN END;
02600 FOR I←1 STEP 1 UNTIL 6
02700 DO BEGIN DTH[I]←DTH[I]-AV[I];
02800 AV[I]←DTH[I]+AV[I] END;
02900 IF ARM_EXECUTE
03000 THEN BEGIN ARM_MESSAGE[21]←'5000000;
03100 ARRBLT(ARM_MESSAGE[1],DTH[1],6) END
03200 ELSE BEGIN ARRTRAN(LAST_ARM,AV);
03300 IF PTR1+PTR4+PTR3+25>PTR2 THEN FLUSH(0,TT);
03400 STACK[PTR3←PTR3+1]←'5000000+(PTR4←PTR4+1);
03500 RELOC[PTR3]←-1;
03600 ARRBLT(DATA_STACK[PTR4],DTH[1],6) END;
03700 SCHEINMAN(EXF,AV);
03800 IF ARM_EXECUTE
03900 THEN ARRBLT(ARM_MESSAGE[7],EXF[1],13)
04000 ELSE BEGIN ARRBLT(DATA_STACK[PTR4←PTR4+6],EXF[1],13);
04100 PTR4←PTR4+12 END;
04200 RESET_CONO;
04300 FOR I←1 STEP 1 UNTIL 6 DO IF(J←8*SQRT(ABS(DTH[I])/MA[I]))>TIME THEN TIME←J;
04400 IF ARM_EXECUTE
04500 THEN BEGIN ARM_MESSAGE[20]←TIME;
04600 ARMFN(20);
04700 ARM_EXECUTE←FALSE;
04800 UPDATE_SEG END
04900 ELSE BEGIN DATA_STACK[PTR4←PTR4+1]←TIME;
05000 ARM_TIME←ARM_TIME+TIME;
05100 ARRTRAN(LAST_TRANS,TT);
05200 FLAG←IF IS_NOT_CLEAR(LAST_TRANS,DTH)THEN 0 ELSE -1 END;
05300 END;
05400
00100 MP SIMPLE PROCEDURE CENTER_HAND(REAL DIST);
00200 BEGIN INTEGER J,N;
00300 SAFE OWN REAL ARRAY DIR[1:4];
00400 SAFE OWN REAL ARRAY DTH[1:6];
00500 IF ARM_EXECUTE THEN FOR J←1 STEP 1 UNTIL 3 DO DIR[J]←ARM_LINK[6,J,2]
00600 ELSE FOR J←1 STEP 1 UNTIL 3 DO DIR[J]←LAST_TRANS[J,2]; DIR[4]←1;
00700 INCREMENT(DTH,DIR,FALSE);
00800 FOR I←1 STEP 1 UNTIL 6 DO DTH[I]←DTH[I];
00900 IF ARM_EXECUTE
01000 THEN BEGIN ARM_MESSAGE[8]←'12000000;
01100 ARRBLT(ARM_MESSAGE[1],DIST,1);
01200 ARRBLT(ARM_MESSAGE[2],DTH[1],6);
01300 ARMFN(7);
01400 ARM_EXECUTE←FALSE;
01500 UPDATE_SEG END
01600 ELSE BEGIN STACK[PTR3←PTR3+1]←'12000000+(PTR4←PTR4+1);
01700 RELOC[PTR3]←-1;
01800 ARRBLT(DATA_STACK[PTR4],DIST,1);
01900 ARRBLT(DATA_STACK[PTR4←PTR4+1],DTH[1],6);
02000 PTR4←PTR4+5 END;
02100 END;
02200
02300 MP SIMPLE PROCEDURE SET_ARM(INTEGER CELL;REAL ARRAY DX_DY_DZ);
02400 BEGIN
02500 REDUCE(DX_DY_DZ);
02600 IF ARM_EXECUTE THEN BEGIN
02700 ARM_MESSAGE[5]←'13000000;
02800 ARM_MESSAGE[1]←CELL;
02900 ARRBLT(ARM_MESSAGE[2],DX_DY_DZ[1],3);
03000 ARMFN(4);
03100 ARM_EXECUTE←FALSE;
03200 UPDATE_SEG END
03300 ELSE BEGIN
03400 STACK[PTR3←PTR3+1]←'13000000+(PTR4←PTR4+1);
03500 RELOC[PTR3]←-1;
03600 DATA_STACK[PTR4]←CELL;
03700 ARRBLT(DATA_STACK[PTR4←PTR4+1],DX_DY_DZ[1],3);
03800 PTR4←PTR4+2 END;
03900 END"SET_ARM";
04000
00100 MP SIMPLE PROCEDURE DRIVE_ARM(INTEGER JOINT;REAL DEG;INTEGER TIME;REFERENCE INTEGER FLAG);
00200 BEGIN INTEGER I,J,N;
00300 REAL R;
00400 SAFE OWN REAL ARRAY EXF[1:13];
00500 SAFE OWN REAL ARRAY LA,TQ[1:6];
00600 FLAG←FALSE;
00700 IF ARM_EXECUTE THEN ARRBLT(LA[1],ARM_VECTOR[1],6) ELSE ARRTRAN(LA,LAST_ARM);
00800 R←LA[JOINT]+DEG;
00900 IF JOINT<6 ∧ (STOP[JOINT,1]-R)*(R-STOP[JOINT,2])<1 THEN BEGIN ARM_EXECUTE←FALSE;RETURN END;
01000 FLAG←TRUE;
01100 LA[JOINT]←R;
01200 SCHEINMAN(EXF,LA);
01300 IF ARM_EXECUTE
01400 THEN BEGIN ARM_MESSAGE[21]←'5000000;
01500 ARM_MESSAGE[1]←0;ARRBLT(ARM_MESSAGE[2],ARM_MESSAGE[1],5);
01600 ARRBLT(ARM_MESSAGE[JOINT],DEG,1) END
01700 ELSE BEGIN ARRBLT(LAST_TRANS[1,1],T[85],16);
01800 STACK[PTR3←PTR3+1]←'5000000+(PTR4←PTR4+1);
01900 RELOC[PTR3]←-1;
02000 DATA_STACK[PTR4]←0;
02100 ARRBLT(DATA_STACK[PTR4+1],DATA_STACK[PTR4],5);
02200 ARRBLT(DATA_STACK[PTR4+JOINT-1],DEG,1) END;
02300 N←IF TIME>20 THEN TIME ELSE 20;
02400 IF (J←6*SQRT(ABS(DEG)/MA[JOINT]))>N THEN N←J;
02500 IF (J←ABS(DEG)/3)>N THEN N←J;
02600 IF ARM_EXECUTE
02700 THEN BEGIN ARRBLT(ARM_MESSAGE[7],EXF[1],13);
02800 ARM_MESSAGE[19]←GO_WORD[JOINT]+2;
02900 IF FREE_ARM[0,1] THEN FREE_JOINT(ARM_MESSAGE[19]);
03000 ARM_MESSAGE[20]←N;
03100 ARMFN(20);
03200 ARM_EXECUTE←FALSE;
03300 UPDATE_SEG END
03400 ELSE BEGIN ARRBLT(DATA_STACK[PTR4←PTR4+6],EXF[1],13);
03500 PTR4←PTR4+12;
03600 DATA_STACK[PTR4]←GO_WORD[JOINT]+2;
03700 IF FREE_ARM[0,1] THEN FREE_JOINT(DATA_STACK[PTR4]);
03800 DATA_STACK[PTR4←PTR4+1]←N;
03900 ARM_TIME←ARM_TIME+N;
04000 ARRTRAN(LAST_ARM,LA) END;
04100 RESET_CONO;
04200 END;
04300
04400 MP SIMPLE PROCEDURE STOP_ARM(REAL ARRAY F,M;REFERENCE INTEGER STAT);
04500 BEGIN SAFE OWN REAL ARRAY TQ,XF[1:6];
04600 LABEL OK;
04700 INTEGER I;
04800 HANDPOS(LAST_ARM);
04900 REDUCE(F);
05000 ARRBLT(XF[1],F[1],3);
05100 REDUCE(M);
05200 ARRBLT(XF[4],M[1],3);
05300 FORCE(TQ,XF);
05400 FOR I←1 STEP 1 UNTIL 6 DO IF ABS(TQ[I]/F0[I])≥0.2 THEN GO TO OK;
05500 STAT←0;
05600 RETURN;
05700 OK: STAT←-1;
05800 STACK[PTR3←PTR3+1]←'7000000+(PTR4←PTR4+1);
05900 RELOC[PTR3]←-1;
06000 ARRBLT(DATA_STACK[PTR4],TQ[1],6);
06100 PTR4←PTR4+5;
06200 END;
06300
06400 MP SIMPLE PROCEDURE MOVE_ARM(REAL ARRAY T;REFERENCE INTEGER FLAG);
06500 BEGIN SAFE OWN REAL ARRAY DTH,J[1:6];
06600 SAFE OWN REAL ARRAY EXF[1:13];
06700 INTEGER K,TIME;
06800 FLAG←0;
06900 IF IS_NOT_CLEAR(T,J) THEN BEGIN ARM_EXECUTE←FALSE; RETURN END;;
07000 FLAG←-1;
07100 IF ARM_EXECUTE THEN BEGIN
07200 FOR I←1 STEP 1 UNTIL 6 DO DTH[I]←J[I]-ARM_VECTOR[I];
07300 WHILE DTH[6]>180.0 DO DTH[6]←DTH[6]-360.0;
07400 WHILE DTH[6]≤-180.0 DO DTH[6]←DTH[6]+360.0;
07500 ARM_MESSAGE[21]←'5000000;
07600 ARRBLT(ARM_MESSAGE[1],DTH[1],6);
07700 SCHEINMAN(EXF,J);
07800 ARRBLT(ARM_MESSAGE[7],EXF[1],13);
07900 TIME←20;
08000 RESET_CONO;
08100 FOR I←1 STEP 1 UNTIL 6 DO IF(K←6*SQRT(ABS(DTH[I])/MA[I]))>TIME THEN TIME←K;
08200 ARM_MESSAGE[20]←TIME;
08300 ARMFN(20);
08400 ARM_EXECUTE←FALSE;
08500 UPDATE_SEG END
08600 ELSE BEGIN
08700 IF TYP_HAND THEN PMAT("TRAJECTORY FROM",LAST_TRANS);
08800 ARRTRAN(LAST_TRANS,T);
08900 IF TYP_HAND THEN PMAT("TRAJECTORY TO",T);
09000 TRAJECTORY(LAST_ARM,J) END;
09100 END;
09200
09300 MP SIMPLE PROCEDURE NO_NULL;NNUL←-1;
09400
00100 MP SIMPLE PROCEDURE DO_IT(STRING FILE);
00200 BEGIN INTEGER I;
00300 ARM_STATUS←0;
00400 ARM_EXECUTE←FALSE;
00500 FOR I←0 STEP 1 UNTIL NEXT_BAND-1 DO IF EQU(FILE,BANDS[I])THEN DONE;
00600 DOIT(TRACK[I],CVSIX(FILE));
00700 IF ARM_STATUS='50 ∧TRACK[I] THEN BANDS[I]←NULL;
00800 UPDATE_SEG;
00900 END;
01000
01100 MP SIMPLE PROCEDURE MERGE_ARM;
01200 BEGIN
01300 IF ARM_EXECUTE ∨ PTR3<2 ∨ STACK[PTR3-1]≠'777777000000 THEN BEGIN ARM_EXECUTE←FALSE; RETURN END;;
01400 STACK[PTR3-1]←(((LNOT STACK[PTR3]) LAND '777777000000) LOR (STACK[PTR3] LAND '777777));
01500 PTR3←PTR3-1;
01600 END;
01700
01800 MP SIMPLE PROCEDURE DO_PROCEED(INTEGER SKIP);
01900 BEGIN SAFE OWN REAL ARRAY DTH[1:6];
02000 ARM_EXECUTE←FALSE;
02100 IF ¬ARM_WAIT THEN RETURN;
02200 ARMPROCEED(SKIP);
02300 UPDATE_SEG;
02400 END;
02500
02600 MP SIMPLE PROCEDURE ARM_CONO(REAL ARRAY ARRIVE,DEPART,OBJECT;INTEGER ARRIVE_TIME,DEPART_TIME);
02700 BEGIN ARRTRAN(ARRIVE_ARM,ARRIVE);
02800 ARRTRAN(DEPART_ARM,DEPART);
02900 OBJECT_MASS←OBJECT[4];
03000 OBJECT_KXX←OBJECT[1];
03100 OBJECT_KYY←OBJECT[2];
03200 OBJECT_KZZ←OBJECT[3];
03300 T1_ARM←DEPART_TIME;
03400 T2_ARM←ARRIVE_TIME;
03500 END;
03600
03700 MP SIMPLE PROCEDURE SET_TOUCH(BOOLEAN STOP_ON_TOUCH);
03800 IF ¬ARM_EXECUTE THEN
03900 STACK[PTR3←PTR3+1]←'6000000+(IF STOP_ON_TOUCH THEN '777777 ELSE 0)
04000 ELSE ARM_EXECUTE←FALSE;
04100
04200 MP SIMPLE PROCEDURE ARM_SKIPE(INTEGER I);
04300 IF ¬ARM_EXECUTE THEN STACK[PTR3←PTR3+1]←'101000000 LOR I ELSE ARM_EXECUTE←FALSE;
04400
04500 MP SIMPLE PROCEDURE ARM_SKIPN(INTEGER I);
04600 IF ¬ARM_EXECUTE THEN STACK[PTR3←PTR3+1]←'103000000 LOR I ELSE ARM_EXECUTE←FALSE;
04700
04800 MP SIMPLE PROCEDURE ARM_SKIPS(INTEGER I);
04900 IF ¬ARM_EXECUTE THEN STACK[PTR3←PTR3+1]←'104000000 LOR I ELSE ARM_EXECUTE←FALSE;
05000
05100 MP SIMPLE PROCEDURE ARM_JMP(INTEGER I);
05200 IF ¬ARM_EXECUTE THEN STACK[PTR3←PTR3+1]←'102000000 LOR ('777777 LAND I) ELSE ARM_EXECUTE←FALSE;
05300
05400 MP SIMPLE MESSAGE PROCEDURE ARM_AOJ(INTEGER I);
05500 IF ¬ARM_EXECUTE THEN STACK[PTR3←PTR3+1]←'16000000 LOR ('777777 LAND I) ELSE ARM_EXECUTE←FALSE;
05600
00100 MP SIMPLE PROCEDURE ARM_SAVE(INTEGER LEVEL);
00200 BEGIN
00300 INTEGER I,J,K;
00400 SAFE OWN REAL ARRAY LA[1:6];
00500 IF ARM_EXECUTE THEN ARRBLT(LA[1],ARM_VECTOR[1],6) ELSE ARRTRAN(LA,LAST_ARM);
00600 HANDPOS(LA);
00700 IF ARM_EXECUTE THEN BEGIN ARM_MESSAGE[20]←'10000000;
00800 ARM_MESSAGE[1]←LEVEL END ELSE
00900 BEGIN STACK[PTR3←PTR3+1]←'10000000+(PTR4←PTR4+1);
01000 RELOC[PTR3]←-1;
01100 DATA_STACK[PTR4]←LEVEL END;
01200 UNDERFLOW(-1);
01300 MMOVE(U1[SQAR(6)],A[SQAR(6)]);
01400 FOR I←5 STEP -1 UNTIL 2 DO
01500 BEGIN MTIMES(U1[SQAR(I)],A[SQAR(I)],U1[SQAR(I+1)]);
01600 MTIMES(U1[SQAR(I+1)],Q[(IF I=2 THEN 17 ELSE 0)],U1[SQAR(I+1)])
01700 END;
01800 A[3]←A[7]←0.0;
01900 MMOVE(A[0],A[0]);
02000 MTIMES(U1[0],A[0],U1[SQAR(2)]);
02100 MTIMES(U1[SQAR(2)],Q[0],U1[SQAR(2)]);
02200 FOR I←2 STEP 1 UNTIL 6 DO MTIMES(U1[SQAR(I)],T[SQAR(I-1)],U1[SQAR(I)]);
02300 MTIMES(U1[0],Q[0],U1[0]);
02400 K←0;
02500 FOR J←11,7,3 DO
02600 FOR I←6 STEP -1 UNTIL 1 DO
02700 REAL_DATA[K←K+1]←U1[SQAR(I)+J]*(IF I=3 THEN 1.0 ELSE 1.0/RAD);
02800 IF ARM_EXECUTE THEN ARRBLT(ARM_MESSAGE[2],REAL_DATA[1],18)
02900 ELSE ARRBLT(DATA_STACK[PTR4←PTR4+1],REAL_DATA[1],18);
03000 UNDERFLOW(0);
03100 IF ARM_EXECUTE THEN BEGIN ARMFN(19);
03200 ARM_EXECUTE←FALSE;
03300 UPDATE_SEG END ELSE PTR4←PTR4+18;
03400 END;
03500
03600 MP SIMPLE MESSAGE PROCEDURE ARM_RESTORE(INTEGER LEVEL);
03700 BEGIN
03800 INTEGER I,J,K;
03900 SAFE OWN REAL ARRAY DIR[1:4];
04000 SAFE OWN REAL ARRAY LA[1:6];
04100 IF ARM_EXECUTE ∨ STACK[PTR3]≥0 THEN RETURN;
04200 HANDPOS(LAST_ARM);
04300 STACK[PTR3+1]←STACK[PTR3];
04400 RELOC[PTR3+1]←RELOC[PTR3];
04500 STACK[PTR3]←'11000000+(PTR4←PTR4+1);
04600 RELOC[PTR3]←-1;
04700 PTR3←PTR3+1;
04800 DATA_STACK[PTR4]←LEVEL;
04900 DIR[4]←1.0;
05000 FOR I←1 STEP 1 UNTIL 3 DO BEGIN
05100 FOR J←1 STEP 1 UNTIL 3 DO DIR[J]←IF J=I THEN 1.0 ELSE 0.0;
05200 INCREMENT(LA,DIR,I>1);
05300 FOR J←1 STEP 1 UNTIL 6 DO
05400 REAL_DATA[I-3+(3*J)]←LA[J] END;
05500 ARRBLT(DATA_STACK[PTR4←PTR4+1],REAL_DATA[1],18);
05600 PTR4←PTR4+18;
05700 END;
05800
05900 MP SIMPLE MESSAGE PROCEDURE SLAVE_ARM;
06000 BEGIN
06100 INTEGER I,J,K;
06200 SAFE OWN REAL ARRAY DIR[1:4];
06300 SAFE OWN REAL ARRAY LA[1:6];
06400 IF ARM_EXECUTE THEN ARRBLT(LA[1],ARM_VECTOR[1],6) ELSE ARRTRAN(LA,LAST_ARM);
06500 HANDPOS(LA);
06600 IF ARM_EXECUTE THEN ARM_MESSAGE[19]←'17000000 ELSE
06700 BEGIN STACK[PTR3←PTR3+1]←'17000000+(PTR4←PTR4+1);
06800 RELOC[PTR3]←-1 END;
06900 DIR[4]←1.0;
07000 FOR I←1 STEP 1 UNTIL 3 DO BEGIN
07100 FOR J←1 STEP 1 UNTIL 3 DO DIR[J]←IF J=I THEN 1.0 ELSE 0.0;
07200 INCREMENT(LA,DIR,I>1);
07300 FOR J←1 STEP 1 UNTIL 6 DO
07400 REAL_DATA[I-3+(3*J)]←LA[J] END;
07500 IF ARM_EXECUTE THEN BEGIN
07600 ARRBLT(ARM_MESSAGE[1],REAL_DATA[1],18);
07700 ARMFN(18);
07800 ARM_EXECUTE←FALSE;
07900 UPDATE_SEG;
08000 END ELSE
08100 BEGIN
08200 ARRBLT(DATA_STACK[PTR4],REAL_DATA[1],18);
08300 PTR4←PTR4+17 END;
08400 END;
08500